package com.dji.activationDemo.messaging;

import com.dji.activationDemo.Constant;
import com.dji.activationDemo.uitls.RandomNum;
import com.dji.activationDemo.uitls.WriteLog;
import com.dji.activationDemo.websocket.WsManager;

import org.json.JSONArray;
import org.json.JSONException;
import org.json.JSONObject;

import java.util.ArrayList;
import java.util.List;

import dji.common.error.DJIError;
import dji.common.mission.waypoint.Waypoint;
import dji.common.mission.waypoint.WaypointAction;
import dji.common.mission.waypoint.WaypointActionType;
import dji.common.mission.waypoint.WaypointMission;
import dji.common.mission.waypoint.WaypointMissionDownloadEvent;
import dji.common.mission.waypoint.WaypointMissionExecutionEvent;
import dji.common.mission.waypoint.WaypointMissionFinishedAction;
import dji.common.mission.waypoint.WaypointMissionFlightPathMode;
import dji.common.mission.waypoint.WaypointMissionGotoWaypointMode;
import dji.common.mission.waypoint.WaypointMissionHeadingMode;
import dji.common.mission.waypoint.WaypointMissionState;
import dji.common.mission.waypoint.WaypointMissionUploadEvent;
import dji.common.mission.waypoint.WaypointTurnMode;
import dji.common.model.LocationCoordinate2D;
import dji.common.util.CommonCallbacks;
import dji.sdk.mission.MissionControl;
import dji.sdk.mission.waypoint.WaypointMissionOperator;
import dji.sdk.mission.waypoint.WaypointMissionOperatorListener;

public class CustomWaypointMission {

    private WaypointMissionOperator waypointMissionOperator = MissionControl.getInstance().getWaypointMissionOperator();

    RandomNum randomNum = new RandomNum();
    JSONObject waypointJSON = new JSONObject();

    JSONArray waypointList = new JSONArray();
    //规划航点任务次数
    int num;
    int taskLength;
    int start = 0;
    int end;

    String sign;
    String missionName;
    String sendName;

    //加载任务
    private void loadMission(WaypointMission waypointMission, String sign) {
        Constant.waypointStatus = "任务加载";
        if (waypointMission != null) {
            DJIError djiError = waypointMissionOperator.loadMission(waypointMission);
            if (djiError == null) {
                uploadMission(sign);
                System.out.println("Mission is loaded successfully");
            } else {
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"加载任务失败：" + djiError.getDescription() + "\"}}");
                WriteLog.writeLog(sign+":加载任务失败："+djiError.getDescription());
                Constant.waypointStatus = "无任务";
            }
        } else {
            sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"未知错误，未收到点\"}}");
            WriteLog.writeLog(sign+":未知错误，未收到点");
            Constant.waypointStatus = "无任务";
        }
    }

    //上传任务
    private void uploadMission(String sign) {
        Constant.waypointStatus = "任务上传";
        if (WaypointMissionState.READY_TO_RETRY_UPLOAD.equals(waypointMissionOperator.getCurrentState()) || WaypointMissionState.READY_TO_UPLOAD.equals(waypointMissionOperator.getCurrentState())) {
            waypointMissionOperator.uploadMission(new CommonCallbacks.CompletionCallback() {
                @Override
                public void onResult(DJIError djiError) {
                    if (djiError == null) {
                        System.out.println("任务上传成功");
                        startMission(sign);
                    } else {
                        sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"上传任务失败：" + djiError.getDescription() + "\"}}");
                        WriteLog.writeLog(sign+":上传任务报错："+ djiError.toString());
                        System.out.println("上传任务报错：" + djiError.toString());
                        Constant.waypointStatus = "无任务";
                    }
                }
            });
        } else {
            System.out.println("Wait for mission to be loaded");
            Constant.waypointStatus = "无任务";
        }
    }

    //开始执行任务
    private void startMission(String sign) {
        boolean up = true;
        while (up){
            if (WaypointMissionState.READY_TO_EXECUTE.equals(waypointMissionOperator.getCurrentState())) {
                up = false;
                System.out.println("任务上传完成，开始执行");
            } else {
                System.out.println("等待上传完成");
                try {
                    Thread.sleep(4000);
                } catch (InterruptedException e) {
                    e.printStackTrace();
                }
            }
        }
        Constant.waypointStatus = "执行任务";

        waypointMissionOperator.startMission(new CommonCallbacks.CompletionCallback() {
            @Override
            public void onResult(DJIError djiError) {
                if (djiError != null) {
                    sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"执行报错：" + djiError.getDescription() + "\"}}");
                    WriteLog.writeLog(sign+"：执行报错:" + djiError.toString());
                    System.out.println("执行报错:" + djiError.toString());
                    Constant.waypointStatus = "无任务";
                } else {
                    sendMessage(sendName + "*" + sign + "*0*{code:\"received\"}");
                    WriteLog.writeLog(sign+"：开始执行任务");
                    System.out.println("开始执行任务");
                    Constant.waypointStatus = "任务中";
                }
            }
        });

    }

    public void setUpListener() {

        Constant.waypointMissionOperatorListener = new WaypointMissionOperatorListener() {
            @Override
            public void onDownloadUpdate(WaypointMissionDownloadEvent waypointMissionDownloadEvent) {
            }

            @Override
            public void onUploadUpdate(WaypointMissionUploadEvent waypointMissionUploadEvent) {
            }

            @Override
            public void onExecutionUpdate(WaypointMissionExecutionEvent waypointMissionExecutionEvent) {
                int n = waypointMissionExecutionEvent.getProgress().targetWaypointIndex;
                boolean isWR = waypointMissionExecutionEvent.getProgress().isWaypointReached;
                WriteLog.writeLog("第" + n + "个航点，飞行状态：" + waypointMissionExecutionEvent.getProgress().executeState.toString() + ",isWaypointReached:" + isWR);
            }

            @Override
            public void onExecutionStart() {
                sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务开始");
                System.out.println("监听到航点任务开始");
            }

            @Override
            public void onExecutionFinish(DJIError djiError) {
                if (djiError == null) {
                    if (Constant.waypointExecution) {
                        System.out.println("任务还剩次数：" + num);
                        if (num == 0) {
                            System.out.println("航点结束");
                            sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务结束");
                            Constant.waypointStatus = "无任务";
                        } else {
                            sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*准备开始下一个小任务，任务还剩次数：" + num);
                            nextTask();
                        }
                    } else {
                        System.out.println("航线任务结束");
                        sendMessage(sendName + "*" + randomNum.genRandomNum() + "*0*航线任务结束");
                        Constant.waypointStatus = "无任务";
                    }
                } else {
                    System.out.println("结束失败:" + djiError.getDescription());
                }

            }
        };
        waypointMissionOperator.addListener(Constant.waypointMissionOperatorListener);
    }

    //校验加载点
    private boolean checkLoadMission() {
        System.out.println("开始：" + start + ",结束：" + end);
        WaypointMission waypointMission = null;
        if (missionName.equals("customFlight")) {
            waypointMission = createWaypointMission();
        }
        if (waypointMission != null) {
            DJIError djiError = waypointMissionOperator.loadMission(waypointMission);
            if (djiError == null) {
                num--;
                if (num > 0) {
                    if (num == 1) {
                        start = end + 1;
                        end = waypointList.length();
                    } else {
                        start = end + 1;
                        end = end + taskLength;
                    }
                    checkLoadMission();
                }
            } else {
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"：第" + start + "个点到" + end + "个点的" + "校验加载任务报错了:" + djiError.getDescription() + "\"}}");
                WriteLog.writeLog(sign + "：第" + start + "个点到" + end + "个点的" + "校验加载任务报错了:" + djiError.getDescription());
                Constant.waypointStatus = "无任务";
                return false;
            }
        } else {
            Constant.waypointStatus = "无任务";
            return false;
        }
        return true;
    }


    public void nextTask() {
        System.out.println("准备下一次任务");
        if (num == 1) {
            start = end + 1;
            end = waypointList.length();
            num--;
        } else {
            start = end + 1;
            end = end + taskLength;
            num--;
        }
        WaypointMission waypointMission = null;
        if (missionName.equals("customFlight")) {
            waypointMission = createWaypointMission();
        }
        loadMission(waypointMission, sign);
    }

    private void toVariable() {
        try {
            if (waypointList.length() > 0 || waypointList != null) {
                waypointList = new JSONArray();
            }
            waypointList = waypointJSON.getJSONArray("data");
            System.out.println("航点数量：" + waypointList.length());
            num = (int) Math.ceil((waypointList.length()) / (double) 99);
            System.out.println("任务次数：" + num);


        } catch (JSONException e) {
            WriteLog.writeLog(sign + "：数据格式有误:" + e.getMessage());
            sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"数据格式有误：" + e.getMessage() + "\"}}");
            Constant.waypointStatus = "无任务";
        }
    }


    boolean waypointBuilderStatus = true;

    public WaypointMission.Builder waypointBuilder() {
        waypointBuilderStatus = true;
        WaypointMission.Builder builder = new WaypointMission.Builder();
        float as = 2f;
        float ms = 5f;
        boolean emorcsle = false;
        WaypointMissionFinishedAction fa = WaypointMissionFinishedAction.NO_ACTION;
        WaypointMissionFlightPathMode fpm = WaypointMissionFlightPathMode.NORMAL;
        WaypointMissionGotoWaypointMode gfwm = WaypointMissionGotoWaypointMode.SAFELY;
        WaypointMissionHeadingMode hm = WaypointMissionHeadingMode.AUTO;
        boolean sgpre = true;
        try {

            if (waypointJSON.has("AS")) {
                as = (float) waypointJSON.getDouble("AS");
            }
            if (waypointJSON.has("MS")) {
                ms = (float) waypointJSON.getDouble("MS");
            }
            if (waypointJSON.has("EMORCSLE")) {
                switch (waypointJSON.getInt("EMORCSLE")) {
                    case 0:
                        emorcsle = true;
                        break;
                    case 1:
                        emorcsle = false;
                        break;
                }
            }
            if (waypointJSON.has("FA")) {
                switch (waypointJSON.getInt("FA")) {
                    case 0:
                        fa = WaypointMissionFinishedAction.NO_ACTION;
                        break;
                    case 1:
                        fa = WaypointMissionFinishedAction.GO_HOME;
                        break;
                    case 2:
                        fa = WaypointMissionFinishedAction.AUTO_LAND;
                        break;
                    case 3:
                        fa = WaypointMissionFinishedAction.GO_FIRST_WAYPOINT;
                        break;
                    case 4:
                        fa = WaypointMissionFinishedAction.CONTINUE_UNTIL_END;
                        break;
                }
            }
            if (waypointJSON.has("FPM")) {
                switch (waypointJSON.getInt("FPM")) {
                    case 0:
                        fpm = WaypointMissionFlightPathMode.NORMAL;
                        break;
                    case 1:
                        fpm = WaypointMissionFlightPathMode.CURVED;
                        break;
                }
            }
            if (waypointJSON.has("GFWM")) {
                switch (waypointJSON.getInt("GFWM")) {
                    case 0:
                        gfwm = WaypointMissionGotoWaypointMode.SAFELY;
                        break;
                    case 1:
                        gfwm = WaypointMissionGotoWaypointMode.POINT_TO_POINT;
                        break;
                }
            }
            if (waypointJSON.has("HM")) {
                switch (waypointJSON.getInt("HM")) {
                    case 0:
                        hm = WaypointMissionHeadingMode.AUTO;
                        break;
                    case 1:
                        hm = WaypointMissionHeadingMode.USING_INITIAL_DIRECTION;
                        break;
                    case 2:
                        hm = WaypointMissionHeadingMode.CONTROL_BY_REMOTE_CONTROLLER;
                        break;
                    case 3:
                        hm = WaypointMissionHeadingMode.USING_WAYPOINT_HEADING;
                        break;
                    case 4:
                        hm = WaypointMissionHeadingMode.TOWARD_POINT_OF_INTEREST;
                        if (waypointJSON.has("SPOI")) {
                            String spoi = waypointJSON.getString("SPOI");
                            String[] spois = spoi.split(",");
                            double slat = Double.parseDouble(spois[0]);
                            double slon = Double.parseDouble(spois[1]);
                            LocationCoordinate2D sp = new LocationCoordinate2D(slat, slon);
                            builder.setPointOfInterest(sp);
                        } else {
                            waypointBuilderStatus = false;
                            sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"数据格式有误：没有兴趣点\"}}");
                            Constant.waypointStatus = "无任务";
                        }
                        break;
                }
            }
            if (waypointJSON.has("SGPRE")) {
                switch (waypointJSON.getInt("SGPRE")) {
                    case 0:
                        sgpre = true;
                        break;
                    case 1:
                        sgpre = false;
                        break;
                }
            }
            builder.autoFlightSpeed(as);
            builder.maxFlightSpeed(ms);
            builder.setExitMissionOnRCSignalLostEnabled(emorcsle);
            builder.finishedAction(fa);//任务完成：悬停
            builder.flightPathMode(fpm);//航线模式：直线
            builder.gotoFirstWaypointMode(gfwm);//安全前往航点
            builder.headingMode(hm);//航向模式:与飞行方向一致
            builder.setGimbalPitchRotationEnabled(sgpre);
        } catch (JSONException e) {
            e.printStackTrace();
        }
        return builder;
    }


    //自定义飞行
    public void customFlight(JSONObject contentJSON, String sign, String name) {
        Constant.waypointStatus = "任务开始规划";
        Constant.waypointExecution = true;
        sendName = name;
        missionName = "customFlight";
        start = 0;
        if (Constant.waypointMissionOperatorListener != null) {
            waypointMissionOperator.removeListener(Constant.waypointMissionOperatorListener);
        } else {
            setUpListener();
        }
        if (waypointJSON != null || waypointList.length() > 0) {
            waypointJSON = new JSONObject();
        }
        waypointJSON = contentJSON;

        WaypointMission waypointMission = null;
        this.sign = sign;

        toVariable();


        if (num <= 1) {
            end = waypointList.length();
            waypointMission = createWaypointMission();
            num--;
        } else {
            taskLength = waypointList.length() / num;
            end = taskLength;
            if (checkLoadMission()) {
                num = (int) Math.ceil(waypointList.length() / (double) 99);
                start = 0;
                taskLength = waypointList.length() / num;
                end = taskLength;
                waypointMission = createWaypointMission();
                num--;
            } else {
                return;
            }
        }
        //开始执行任务
        loadMission(waypointMission, sign);
    }

    //规划航点
    private WaypointMission createWaypointMission() {
        WaypointMission.Builder builder = waypointBuilder();
        List<Waypoint> waypoints = new ArrayList<>();
        for (int i = start; i < end; i++) {
            try {
                JSONObject wayJSON = waypointList.getJSONObject(i);
                double baseLongitude = wayJSON.getDouble("lon");
                double baseLatitude = wayJSON.getDouble("lat");
                float baseAltitude = (float) wayJSON.getDouble("height");
                Waypoint waypoint = new Waypoint(baseLatitude, baseLongitude, baseAltitude);
                waypoint.turnMode = WaypointTurnMode.CLOCKWISE;
                if (wayJSON.has("heading")){
                    int heading = wayJSON.getInt("heading");
                    System.out.println(sign+":设置航点heading成功:"+heading);
                    WriteLog.writeLog(sign+":设置航点heading成功:"+heading);
                    waypoint.heading = heading;
                }

                if (wayJSON.has("task")) {
                    JSONArray tasks = wayJSON.getJSONArray("task");
                    for (int j = 0; j < tasks.length(); j++) {
                        String task = tasks.getString(j);
                        String[] tks = task.split("\\|");
                        int caseStatus = Integer.parseInt(tks[0]);
                        int actionParam = Integer.parseInt(tks[1]);
                        switch (caseStatus) {
                            case 0:
                                waypoint.addAction(new WaypointAction(WaypointActionType.STAY, actionParam));
                                break;
                            case 1:
                                waypoint.addAction(new WaypointAction(WaypointActionType.START_TAKE_PHOTO, actionParam));
                                break;
                            case 2:
                                waypoint.addAction(new WaypointAction(WaypointActionType.START_RECORD, actionParam));
                                break;
                            case 3:
                                waypoint.addAction(new WaypointAction(WaypointActionType.STOP_RECORD, actionParam));
                                break;
                            case 4:
                                waypoint.addAction(new WaypointAction(WaypointActionType.ROTATE_AIRCRAFT, actionParam));
                                break;
                            case 5:
                                waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, actionParam));
                                break;
                        }
                    }
                }
//                waypoint.addAction(new WaypointAction(WaypointActionType.GIMBAL_PITCH, -90));
                waypoints.add(waypoint);
            } catch (JSONException e) {
                sendMessage(sendName + "*" + sign + "*0*{code:\"error\",data:{message:\"：第" + i + "个点数据有问题:" + e.getMessage() + "\"}}");
                WriteLog.writeLog(sign + "：第" + i + "个点数据有问题:" + e.getMessage());
                Constant.waypointStatus = "无任务";
                return null;
            }

        }
        builder.waypointList(waypoints).waypointCount(waypoints.size());
        return builder.build();
    }


    //发送消息
    public void sendMessage(String message) {

        new Thread(new Runnable() {
            @Override
            public void run() {
                boolean start = true;
                while (start) {
                    try {
                        if (WsManager.ws.isOpen()) {
                            System.out.println("发送成功：" + message);
                            WsManager.ws.sendText(message);
                            start = false;
                        }
                        Thread.sleep(250);
                    } catch (InterruptedException e) {
                        e.printStackTrace();
                    }

                }
            }
        }).start();
    }
}
